#include "CAN_receive.h"
#include "main.h"

#include "RC_receive.h"


/* 
	我们规定:
		底盘四电机ID依次为CAN1的201~204
		云台yaw轴电机为CAN2的205
*/

extern CAN_HandleTypeDef hcan1;
extern CAN_HandleTypeDef hcan2;

//RM电机接收报文协议
#define get_motor_measure(ptr, data)                                \
{                                                                   \
    (ptr)->last_ecd = (ptr)->ecd;                                   \
    (ptr)->ecd = (uint16_t)((data)[0] << 8 | (data)[1]);            \
    (ptr)->speed_rpm = (uint16_t)((data)[2] << 8 | (data)[3]);      \
    (ptr)->given_current = (uint16_t)((data)[4] << 8 | (data)[5]);  \
    (ptr)->temperate = (data)[6];                                   \
}


motor_measure_t motor_chassis_data[4]; //底盘四电机 M1 M2 M3 M4
motor_measure_t motor_gimbal_data[1];  //云台 YAW
receive_broad_c_t broad_type_c;        //C板对底盘的控制数据

CAN_TxHeaderTypeDef  can1_tx_message;
CAN_TxHeaderTypeDef  can2_tx_message;
uint8_t              can1_send_data[8];
uint8_t              can2_send_data[8];

//C板通讯接收解包
void get_cmd_form_broad_type_c(uint8_t rx_data[8]);

//CAN接收回调函数
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) 
{
    CAN_RxHeaderTypeDef rx_header;
    uint8_t rx_data[8];
    static uint8_t i = 0;

    HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &rx_header, rx_data);
    
    if(hcan == &hcan1)   //CAN1频道
    {
      switch (rx_header.StdId)
      {
        case CAN_3508_M1_ID:
        case CAN_3508_M2_ID:
        case CAN_3508_M3_ID:
        case CAN_3508_M4_ID:
        {
            i = rx_header.StdId - CAN_3508_M1_ID; //获取对应i值
            get_motor_measure(&motor_chassis_data[i], rx_data);
            break;
        }

        default:
        {
          break;
        }
      }
    }

    else if(hcan == &hcan2) //CAN2频道
    {
      switch (rx_header.StdId)
      {
        case CAN_FROM_BROAD_TYPE_C:
        {
            get_cmd_form_broad_type_c(rx_data);
            break;
        }

        case CAN_YAW_MOTOR_ID:
        {
            get_motor_measure(&motor_gimbal_data[0], rx_data);
            break;
        }

        default:
        {
            break;
        }
    }
  }
}

//C板通讯接收解包
void get_cmd_form_broad_type_c(uint8_t rx_data[8])
{
    broad_type_c.chassis_vx = (int16_t)(rx_data[0] << 8 | rx_data[1]);      //chassis x speed
    broad_type_c.chassis_vy = (int16_t)(rx_data[2] << 8 | rx_data[3]);      //chassis y speed
    broad_type_c.chassis_wz = (int16_t)(rx_data[4] << 8 | rx_data[5]);      //chassis w speed
    broad_type_c.chassis_mode = rx_data[6];                                  //chassis mode
}

//C板通讯发送数据
void CAN_send_to_broad_type_c(int16_t shoot_heat) 
{
    uint32_t send_mail_box;
    can2_tx_message.StdId = CAN_SENT_BROAD_TYPE_C;
    can2_tx_message.IDE = CAN_ID_STD;
    can2_tx_message.RTR = CAN_RTR_DATA;
    can2_tx_message.DLC = 0x08;

    can2_send_data[0] = (shoot_heat >> 8);    //枪口热量数据
    can2_send_data[1] = shoot_heat;           
    can2_send_data[2] = 0;
    can2_send_data[3] = 0;
    can2_send_data[4] = 0;
    can2_send_data[5] = 0;
    can2_send_data[6] = 0;
    can2_send_data[7] = 0;

    HAL_CAN_AddTxMessage(&hcan2, &can2_tx_message, can2_send_data, &send_mail_box);
}

//发送底盘电机控制电流CAN1(0x201,0x202,0x203,0x204)
void CAN_cmd_chassis(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4)  
{
    uint32_t send_mail_box;
    can1_tx_message.StdId = CAN_CHASSIS_ALL_ID;
    can1_tx_message.IDE = CAN_ID_STD;
    can1_tx_message.RTR = CAN_RTR_DATA;
    can1_tx_message.DLC = 0x08;
    can1_send_data[0] = motor1 >> 8;
    can1_send_data[1] = motor1;
    can1_send_data[2] = motor2 >> 8;
    can1_send_data[3] = motor2;
    can1_send_data[4] = motor3 >> 8;
    can1_send_data[5] = motor3;
    can1_send_data[6] = motor4 >> 8;
    can1_send_data[7] = motor4;

    HAL_CAN_AddTxMessage(&hcan1, &can1_tx_message, can1_send_data, &send_mail_box);
}

//返回底盘电机 3508电机数据指针 电机编号i,范围[0,3]
const motor_measure_t *get_chassis_motor_measure_point(uint8_t i)
{
    return &motor_chassis_data[(i & 0x03)];
}

//返回yaw 6020电机数据指针
const motor_measure_t *get_yaw_motor_measure_point(void) 
{
    return &motor_gimbal_data[0];
}

//返回C板控制的数据指针
const receive_broad_c_t *get_broad_type_c_data_point(void) 
{
    return &broad_type_c;
}
